#include "vision_service_direct_funcs.h"
#include "vision_service_direct_types.h"

// #include <opencv2/opencv.hpp>

#include <unistd.h>
#include <fstream>
#include <iostream>


#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>

// #include <sstream>
// #include <tf/transform_broadcaster.h>
// #include <message_filters/subscriber.h>
#include <sensor_msgs/image_encodings.h>
#include <std_msgs/Int16.h>
#include <std_msgs/Int16.h>
#include <zhz_msgs/FCVisionSwitchFrame.h>
#include <zhz_msgs/NedFrame.h>
#include <zhz_msgs/RCFrame.h>

sensor_msgs::Imu AttFrameToImuMsg(const AttFrame& att_frame) {
    sensor_msgs::Imu imu_msg;

    // 填充IMU消息的数据
    imu_msg.header.stamp = ros::Time(att_frame.timestamp / 1000000, (att_frame.timestamp % 1000000) * 1000);
    imu_msg.header.frame_id = "imu_frame";
    

    // 设置角速度数据
    imu_msg.angular_velocity.x = att_frame.imu_gyro[0];
    imu_msg.angular_velocity.y = att_frame.imu_gyro[1];
    imu_msg.angular_velocity.z = att_frame.imu_gyro[2];

    // 设置线性加速度数据
    imu_msg.linear_acceleration.x = att_frame.imu_accel[0];
    imu_msg.linear_acceleration.y = att_frame.imu_accel[1];
    imu_msg.linear_acceleration.z = att_frame.imu_accel[2];

    // 设置四元数数据
    imu_msg.orientation.w = att_frame.uav_quat[0];
    imu_msg.orientation.x = att_frame.uav_quat[1];
    imu_msg.orientation.y = att_frame.uav_quat[2];
    imu_msg.orientation.z = att_frame.uav_quat[3];
    
    return imu_msg;
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "msg_convert_imu");
  ros::NodeHandle nh("~");

  std::string imu_topic;
  ros::param::get("~imu_topic", imu_topic);
  ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>(imu_topic, 1, true);
  ros::Rate rate(100);
  int frame_id = 0;
  int ret = 0;
  // 1. open msg receiver
  ret = vision_create_device_stream();  // 0: success; -1: failed
  while(ros::ok()) {
    if (ret) {
        printf("vision_create_device_stream failed.\n");
        rate.sleep();
        continue;
    }
    AttFrame imu_data;
    char str_out[256], str_att[256];
    uint64_t time_now = vision_get_system_time(TimeSystem::TIME_FCS);
    // 3.1 Get One IMU
    ret = vision_get_history_device_frame(DeviceStream::DEVICE_ATT, time_now, 50'000'000ULL, &imu_data);

    sensor_msgs::Imu imu_msg = AttFrameToImuMsg(imu_data);
    imu_msg.header.seq = frame_id++;
    imu_pub.publish(imu_msg);
    ros::spinOnce();
    rate.sleep();
  }
  ros::shutdown();
  exit(0);
  return 0;
}